#pragma once
/* 
* COPYRIGHT 2012 Mafahir Fairoze
* 
* This file is part of Vector Template Library (Vtl) v1.0.
* (Project Website : http://mafahir.wordpress.com/projects/neuralplusplus)
* 
* Vector Template Library (Vtl) v1.0 is a free software. You can redistribute it and/or modify it under the terms of
* the GNU General Public License as published by the Free Software Foundation, either version 3
* of the License, or (at your option) any later version.
* 
* Vector Template Library (Vtl) v1.0 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY;
* without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License <http://www.gnu.org/licenses/> for more details.
*/

#include "Vtl/eigen.h"
#include "Vtl/common.h"

namespace Vtl
{
	template <typename Scalar, int Dimension>
	class AABBox
	{
	public:
		typedef Eigen::Matrix<Scalar, Dimension, 1> VectorType;
		typedef Eigen::Matrix<Scalar, 3, 1> VectorX3d;
		typedef Eigen::ParametrizedLine<Scalar, Dimension> ParametrizedLine;
		typedef Eigen::ParametrizedLine<Scalar, 3> ParametrizedLineX3d;

		/*!
		*  \brief Default constructor.
		*  Sets the bounding box to an empty (uninitialized) state.
		*  \param[in] _Eps (optional) Epsilon, increased value will result reduced accuracy of computation. Defaults to 1e-4.
		*/
		AABBox(const double &_Eps = 1e-4)
			:is_empty(true),eps(_Eps)
		{
		}

		/*!
		*  \brief Constructor.
		*  Sets the bounding box to a NOT empty (initialized) state.
		*  \param[in] _Min minimum edge, has the lowest values of all axis. Should be of the same type(Scalar & Dimension).
		*  \param[in] _Max maximum edge, has the highest values of all axis. Should be of the same type(Scalar & Dimension).
		*  \param[in] _Eps (optional) Epsilon, increased value will result reduced accuracy of computation. Defaults to 1e-4.
		*/
		AABBox(const VectorType &_Min, const VectorType &_Max, const double &_Eps = 1e-4)
			:is_empty(false), min(common::convertTo3D(_Min)), max(common::convertTo3D(_Max)), eps(_Eps)
		{
		}

		/*!
		*  \brief Returns vector between the maximum and minimum edge.
		*  \return VectorX3d 
		*/
		VectorX3d getExtent() const
		{
			return (max - min);
		}

		/*!
		*  \brief Returns the center point of the bounding box.
		*  \return VectorX3d 
		*/
		VectorX3d getCenter() const
		{
			return (max + min)/2.0;
		}

		/*!
		*  \brief Resets the bounding box the given point.
		*  Sets the bounding box to NOT empty (initialized) state.
		*  \param[in] _Point should be of the same type(Scalar & Dimension).
		*/
		void Reset(const VectorType &_Point)
		{
			const VectorX3d point = common::convertTo3D(_Point);
			is_empty = false;
			min = point;
			max = point;
		}

		/*!
		*  \brief Resets the bounding box.
		*  Sets the bounding box to empty (uninitialized) state.
		*/
		void Reset()
		{
			is_empty = true;
			min = max = VectorX3d();
		}

		/*!
		*  \brief Adds the point to expand bounding box.
		*  Only if the given point is outside, the bounding box shall expand.
		*  Or the point will be ignored.
		*  
		*  If the bounding box is in empty state, it will reset the bounding box
		*  to the given point.
		*  \param[in] _Point should be of the same type(Scalar & Dimension).
		*/
		void AddInPoint(const VectorType &_Point)
		{
			if(!is_empty)
			{
				const VectorX3d point = common::convertTo3D(_Point);
				if(point(0) > max(0)) max(0) = point(0) + eps;
				if(point(0) < min(0)) min(0) = point(0) - eps;

				if(point(1) > max(1)) max(1) = point(1) + eps;
				if(point(1) < min(1)) min(1) = point(1) - eps;

				if(point(2) > max(2)) max(2) = point(2) + eps;
				if(point(2) < min(2)) min(2) = point(2) - eps;
			}
			else
			{
				Reset(_Point);
			}
		}

		/*!
		*  \brief Checks if the given point is inside the bounding box.
		*  \param[in] _Point should be of the same type(Scalar & Dimension).
		*  \return bool returns TRUE if inside.
		*/
		bool Contains(const VectorType &_Point) const
		{
			const VectorX3d point = common::convertTo3D(_Point);
			return (point(0) >= min(0) && point(0) <= max(0) &&
				point(1) >= min(1) && point(1) <= max(1) &&
				point(2) >= min(2) && point(2) <= max(2));
		}

		/*!
		*  \brief Checks if the given bounding box intersects.
		*  \param[in] _Box should be of the same type(Scalar & Dimension).
		*  \return bool returns TRUE if it does intersect.
		*/
		bool IntersectWithBox(const AABBox &_Box) const
		{
			return (min(0) <= _Box.max(0) && min(1) <= _Box.max(1) && min(2) <= _Box.max(2) &&
				max(0) >= _Box.min(0) && max(1) >= _Box.min(1) && max(2) >= _Box.min(2));
		}

		/*!
		*  \brief Checks if the given line intersects.
		*  \param[in] _Line should be of the same type(Scalar & Dimension).
		*  \return bool returns TRUE if it does intersect.
		*/
		bool IntersectWithLine(const ParametrizedLine &_Line) const
		{
			const ParametrizedLineX3d line = common::convertTo3D(_Line);
			const VectorX3d linevect = line.direction().normalized();
			const Scalar halflength = line.direction().norm() * Scalar(0.5);
			const VectorX3d e = getExtent() * Scalar(0.5);
			const VectorX3d t = getCenter() - (line.origin() + (line.direction() * Scalar(0.5)));

			if ((fabsl(t(0)) > e(0) + halflength * fabsl(linevect(0))) ||
				(fabsl(t(1)) > e(1) + halflength * fabsl(linevect(1))) ||
				(fabsl(t(2)) > e(2) + halflength * fabsl(linevect(2))) )
				return false;

			Scalar r = e(1) * Scalar(fabsl(linevect(2))) + e(2) * Scalar(fabsl(linevect(1)));
			if (fabsl(t(1)*linevect(2) - t(2)*linevect(1)) > r )
				return false;

			r = e(0) * Scalar(fabsl(linevect(2))) + e(2) * Scalar(fabsl(linevect(0)));
			if (fabsl(t(2)*linevect(0) - t(0)*linevect(2)) > r )
				return false;

			r = e(0) * Scalar(fabsl(linevect(1))) + e(1) * Scalar(fabsl(linevect(0)));
			if (fabsl(t(0)*linevect(1) - t(1)*linevect(0)) > r)
				return false;

			return true;
		}

	protected:
		VectorX3d min;
		VectorX3d max;
		bool is_empty;
		double eps;
	};

	typedef AABBox<double, 3> AABBox3d;
	typedef AABBox<double, 2> AABBox2d;
	typedef AABBox<float, 3> AABBox3f;
	typedef AABBox<float, 2> AABBox2f;
	typedef AABBox<int, 3> AABBox3i;
	typedef AABBox<int, 2> AABBox2i;
};